import sys
import time

import piper_sdk
from piper_sdk import C_PiperInterface_V2

if __name__ == "__main__":
    if len(sys.argv) != 2:
        print("Usage: python go_zero.py <can_id>")
        sys.exit(1)

    piper = C_PiperInterface_V2(sys.argv[1])
    piper.ConnectPort()
    while not all(piper.GetArmEnableStatus()):
        piper.EnableArm(7)
        time.sleep(0.01)
    piper.ModeCtrl(0x01, 0x01, 100, 0x00)
    piper.JointCtrl(0, 0, 0, 0, 0, 0)
    piper.GripperCtrl(0, 1000, 0x01, 0)
